/*
 * anima/x86_64/init32.cc - 32-bit portion of AP initialization.
 * Copyright (C) 2007  Mikhail Vorozhtsov
 * See the LICENSE section of the README file for details.
 */

/* $Id$ */

#include HEADER (anima,locks.hh)
#include HEADER (x86_64,regs.hh)
#include HEADER (x86_64,msrs.hh)
#include HEADER (x86_64,cpuid.hh)
#include HEADER (x86_64,descs.hh)
#include HEADER (x86_64,mmu.hh)

START_NAMESPACE2 (anima,x86_64)

static ::x86_64::desc_t gdt[2];

static ::x86_64::pe_t pml4[512] ALIGNED (4096);
static ::x86_64::pe_t pdp[512] ALIGNED (4096);
static ::x86_64::pde2m_t pd[512] ALIGNED (4096);

static ::anima::spin_lock32_t lock;

extern "C" void
init32_ap (void *init_ap)
{
  lock.lock ();

  // Test if we have CPUID.
  {
    ::x86_64::flags_t flags = ::x86_64::flags_t::get ();
    word_t saved_id = flags.ID;

    flags.ID = !flags.ID;
    flags.set ();

    if (::x86_64::flags_t::get ().ID == saved_id)
      {
        lock.unlock ();
        return;
      }
  }

  // Test if Long Mode is supported.
  {
    if (::x86_64::cpuid_ext_feats_t::get_max_fn ()
        < ::x86_64::cpuid_ext_feats_t::FN1)
      {
        lock.unlock ();
        return;
      }

    ::x86_64::cpuid_ext_feats_t feats = ::x86_64::cpuid_ext_feats_t::get ();

    if (!feats.LM)
      {
        lock.unlock ();
        return;
      }
  }

  // Set up page table.
  {
    static bool first_p = true;

    if (first_p)
      {
        // Fill in page directory.
        for (int i = 0; i < 512; ++i)
          pd[i].set_base (i).set_PS ().set_RW ().set_P ();

        // First PDP entry must point to PD, others must not be present.
        pdp[0].set_ptr ((u32_t) pd).set_RW ().set_P ();

        // First PML4 entry must point to PDP, others must not be present.
        pml4[0].set_ptr ((u32_t) pdp).set_RW ().set_P ();

        first_p = false;
      }
  }

  // Enable PAE.
  ::x86_64::cr4_t::get ().set_PAE ().set ();

  // Enable Long Mode.
  ::x86_64::efer_t::get ().set_LME ().set ();

  // Enable paging.
  ::x86_64::cr3_t::get ().set_ptr ((u32_t) pml4).set ();
  ::x86_64::cr0_t::get ().set_WP ().set_PG ().set ();

  // Test if Long Mode is active.
  if (!::x86_64::efer_t::get ().LMA)
    {
      lock.unlock ();
      return;
    }

  /* We are in Compatability Mode. */

  // Set up temprorary GDT for far jump.
  {
    static bool first_p = true;

    if (first_p)
      {
        gdt[0].set_type (::x86_64::desc_t::TYPE_DATA)
          .set_S ().set_P ().set_D ();
        gdt[1].set_type (::x86_64::desc_t::TYPE_CODE)
          .set_S ().set_P ().set_L ();
        first_p = false;
      }
  }

  ::x86_64::set_gdt ((u64_t) (u32_t) gdt, sizeof (gdt));

  lock.unlock ();

  // Leave.
  asm volatile (
    // Far jump to 64-bit code segment.
    ".byte 0xEA\n"
    ".long 1f\n"
    ".word 0x08\n"
    "1:\n"
    // We are in Long Mode.
    ".code64\n"
    // Jump to 64-bit C code.
    "jmp *%%rax\n"
    // Halt on failure.
    "hlt\n"
    ".code32\n"
    :
    : "a"(init_ap)
  );
}

END_NAMESPACE2

